﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Primitives;

namespace Drifting.MotionPlanning.PointSets
{
    public class AlphaRootQuery : LinearQuery
    {
        protected Random rand;
        protected double alpha;

        public AlphaRootQuery(Random rand, double alpha)
        {
            this.rand = rand;

            if (alpha >= 1 && alpha <= 2)
                this.alpha = 1 / alpha;
            else
                throw new ArgumentException("alpha must be in [1, 2]");
        }

        override public void Neighbor_SingleQuery(double[] q, ref State x_nearest, ref double dist)
        {
            int idx_start = rand.Next(pointList.Count);
            int step_size = (int)(pointList.Count / Math.Pow(pointList.Count, alpha));

            double dist_min = double.MaxValue;
            State x_min = null;

            for (int i = idx_start; i < pointList.Count; i+=step_size)
            {
                State x_i = pointList[i];
                double dist_i = Euclidean2(x_i.q, q);
                if (dist_i < dist_min)
                {
                    dist_min = dist_i;
                    x_min = x_i;
                }
            }
            for (int i = idx_start - step_size; i >= 0; i -= step_size)
            {
                State x_i = pointList[i];
                double dist_i = Euclidean2(x_i.q, q);
                if (dist_i < dist_min)
                {
                    dist_min = dist_i;
                    x_min = x_i;
                }
            }

            x_nearest = x_min;
            dist = dist_min;
        }
    }
}
